#include "stanley_lateral_kinematics.h"
StartStanleyNs
StanleyLateral::StanleyLateral(double wheel_base ,double forward_gain, double backward_gain):
    wheel_base_(wheel_base),forward_gain_(forward_gain),backward_gain_(backward_gain),is_forward_(true)
{

}
bool StanleyLateral::step()
{   
    if (speed_mps_<0)
    {
        std::cout<<"speed_mps must be positive or zero"<<std::endl;
        return false;
    }
    delta_theta_ =current_pose_.theta -ref_pose_.theta;             
    if(is_forward_)
    {
        delta_x_g_=wheel_base_*std::cos(current_pose_.theta*deg_to_rad)+
            current_pose_.x -ref_pose_.x;
        delta_y_g_ = wheel_base_*std::sin(current_pose_.theta*deg_to_rad)+
            current_pose_.y -ref_pose_.y;
        lateral_error_ = -delta_x_g_*std::sin(ref_pose_.theta*deg_to_rad)+
            delta_y_g_*std::cos(ref_pose_.theta*deg_to_rad);
        longtitude_error_=delta_x_g_*std::cos(ref_pose_.theta*deg_to_rad)+
            delta_y_g_*std::sin(ref_pose_.theta*deg_to_rad);
        tire_angle_ = -std::atan( lateral_error_*forward_gain_/(1.0+speed_mps_) )
            /deg_to_rad-delta_theta_;
    }
    else
    {
        delta_x_g_ =current_pose_.x -ref_pose_.x;
        delta_y_g_ = current_pose_.y -ref_pose_.y;
        lateral_error_ = -delta_x_g_*std::sin(ref_pose_.theta*deg_to_rad)+
            delta_y_g_*std::cos(ref_pose_.theta*deg_to_rad);
        tire_angle_ = -std::atan( lateral_error_*backward_gain_/(1.0+speed_mps_) )
            /deg_to_rad+delta_theta_;
    
    }
    if(tire_angle_>max_tire_angle)
    {
        tire_angle_ = max_tire_angle;
    }
    else if(tire_angle_< -max_tire_angle)
    {
        tire_angle_ = -max_tire_angle;
    }
    else
    {
        /* code */
    }
    
    return true;
}
EndStanleyNs